#include "common/linkage.h"
#include "linux/console.h"
#include "i386/kernel/io.h"
#include "lib/vsprintf.h"
#include <stdarg.h>

static int current_ypos = 1;
static int current_xpos = 0;

// #define VGABASE (__ISA_IO_base + 0xb8000)
#define VGABASE (0xb8000)

#define MAX_YPOS 25
#define MAX_XPOS 80

static void early_vga_write(struct console *con, const char* str, u32 n) {
  char c;
  int i, j, k;

  while ((c = *str++) != '\0' && n-- > 0) {
    // 如果大于最大一行，那么向上卷一行
    if (current_ypos >= MAX_YPOS) {
      for (k = 1, j = 0;  k < MAX_YPOS; k++, j++) {
        for (i = 0; i < MAX_XPOS; i++) {
          writew(readw((volatile void __iomem*)(VGABASE + 2 * (MAX_XPOS*k + i))),
                 (volatile void __iomem*)(VGABASE + 2 * (MAX_XPOS * j + i)));
        }
      }

      for (i = 0; i < MAX_XPOS; ++i) {
        writew(0x720, (volatile void __iomem*)(VGABASE + 2 * (MAX_XPOS *j + 1)));
      }
      current_ypos = MAX_YPOS - 1;
    }

    if (c == '\n') {
      current_xpos = 0;
      current_ypos++;
    } else if (c != '\r') {
      writew(((0x7 << 8) | (unsigned short)c),
             (volatile void __iomem*)(VGABASE + 2 * (MAX_XPOS * current_ypos + current_xpos++)));
      if (current_xpos >= MAX_XPOS) {
        current_xpos = 0;
        current_ypos++;
      }
    }
  }
}

static struct console early_vga_console = {
  .name = "earlyvga",
  .write = early_vga_write,
  .flags = 0,
  .index = -1,
};

struct console *early_console = &early_vga_console;
static int early_console_initialized = 0;

int __init setup_early_printk(char* opt) {
  early_console_initialized = 1;
  return 0;
}

void __init disable_early_printk(void) {
}

void early_printk(const char* fmt, ...) {
  char buf[512];
  int n;
  va_list ap;

  va_start(ap, fmt);
  n = vscnprintf(buf, 512, fmt, ap);
  early_console->write(early_console, buf, n);
  va_end(ap);
}
